#include "FOC_transform.h"
#include "at32f415_wk_config.h"
#include "wk_system.h"
#include "MotorDriver.h"
#include "flash.h"

// 角度校准参数
volatile const float theta_offset = 0;
#define theta_offset_addr (0x08000000 + 1024 * 60) // 偏移角度存储地址
float theta_offset_temp = -0.11;
#define erro_limit 0.015f // 误差限制 在 1度 以内

/**
 * @brief FOC中的Clark变换函数，实现三相坐标系(ABC)与两相静止坐标系(Alpha-Beta)之间的变换
 *
 * @param inverse 变换方向，
 *          TRANFORM_INVERSE 0 表示逆变换 ABC <- Alpha Beta
 *          TRANFORM_FORWARD 1 表示正变换 Alpha Beta <- ABC
 * @param a 指向A相分量的指针
 * @param b 指向B相分量的指针
 * @param c 指向C相分量的指针
 * @param alpha 指向Alpha轴分量的指针
 * @param beta 指向Beta轴分量的指针
 * @return uint8_t 返回执行状态，0表示成功，1表示空指针错误，2表示变换方向参数错误
 */
uint8_t FOC_clark_transform(tranform_driction_t inverse, // 0: 逆变换 1: 正变换
                            float *a, float *b, float *c,
                            float *alpha, float *beta)
{
    // 检查输入参数是否为空指针
    if (a == NULL || b == NULL || c == NULL || alpha == NULL || beta == NULL)
        return 1;
    // 检查变换方向参数是否有效
    if (inverse != TRANFORM_INVERSE && inverse != TRANFORM_FORWARD)
        return 2;

    if (inverse == TRANFORM_FORWARD)
    {
        // 正变换 Alpha Beta <- ABC
        *alpha = *a - 0.5f * (*b + *c);
        *beta = 0.866f * (*b - *c);
    }
    else // 逆变换 ABC <- Alpha Beta
    {
        *a = +2 / 3 * *alpha;
        *b = -1 / 3 * *alpha + 1 / sq3 * *beta;
        *c = -1 / 3 * *alpha - 1 / sq3 * *beta;
    }
    return 0;
}

/**
 * @brief FOC帕克变换函数，实现alpha-beta坐标系与d-q坐标系之间的变换
 *
 * @param inverse 变换方向，
 *              TRANFORM_INVERSE 0 表示逆变换 alpha-beta <- d-q
 *              TRANFORM_FORWARD 1 表示正变换 d-q <- alpha-beta
 * @param theta 旋转角度指针
 * @param alpha alpha轴分量指针
 * @param beta beta轴分量指针
 * @param q q轴分量指针
 * @param d d轴分量指针
 *
 * @return uint8_t 执行结果，0表示成功，1表示参数为空，2表示变换方向参数错误
 */
uint8_t FOC_park_transform(tranform_driction_t inverse, // 0: 逆变换 1: 正变换
                           float *theta, float *alpha, float *beta,
                           float *d, float *q)
{
    // 参数有效性检查
    if (theta == NULL || alpha == NULL || beta == NULL || q == NULL || d == NULL)
        return 1;
    if (inverse != TRANFORM_INVERSE && inverse != TRANFORM_FORWARD)
        return 2;

    if (inverse == TRANFORM_FORWARD)
    {
        // 正变换 q d <- alpha beta
        *d = *alpha * cosf(*theta) + *beta * sinf(*theta);
        *q = -*alpha * sinf(*theta) + *beta * cosf(*theta);
    }
    else if (inverse == TRANFORM_INVERSE)
    {
        // 逆变换 alpha beta <- q d
        *alpha = *d * cosf(*theta) - *q * sinf(*theta);
        *beta = *d * sinf(*theta) + *q * cosf(*theta);
    }
    return 0;
}

static float angle_Last;        // 上次角度
static float velocity_Last = 0; // 上次角度(用于计算转速)

void FOC_AngleInit(float angle)
{
    angle_Last = angle;
    velocity_Last = angle;
}

float FOC_angle_normalization(float angle)
{
    // 角度归一化到 [0, 2PI)
    float angle_e;
    angle_e = fmodf(angle, _2PI);
    if (angle_e < 0)
        angle_e += _2PI;
    return angle_e;
}

/*
 * raw_angle: 原始角度 0~2PI
 * full_angle: 全角度 -oo ~ +oo
 */
uint8_t FOC_FullAngle(float *raw_angle, float *full_angle, float *v)
{
    const static float time = 0.005f;   // s 转速计算间隔时间
    static int32_t rotation_Count = 0; // 转过的圈数
    // static int32_t rotation_Count_Last; // 上次转过的圈数

    float deltaAngle = *raw_angle - angle_Last; // 本次角度减去上次角度
    if (fabsf(deltaAngle) > (0.8f * _2PI))
        rotation_Count += (deltaAngle > 0) ? -1 : 1;

    angle_Last = *raw_angle; // 保存本次角度

    *full_angle = *raw_angle + rotation_Count * _2PI; // 全角度

    *v = (*full_angle - velocity_Last) / _2PI / time ; // rad/s 转速
    velocity_Last = *full_angle;
    return 0;
}

// 获取电角度
void FOC_elec_Angle(float *raw_angle_f, float *angle_e)
{
    float raw_angle_f_temp = 0.0f;
    if (raw_angle_f == NULL)
    {
        MT6701_get_angle(NULL, &raw_angle_f_temp); // 获取原始角度
    }
    else
    {
        raw_angle_f_temp = *raw_angle_f; // 使用传入的原始角度
    }

    // 将电角度归一化到 [0, 2PI)
    *angle_e = FOC_angle_normalization(-(*raw_angle_f + theta_offset_temp) * 7.0f);
}

/**
 * @brief 实现空间矢量脉宽调制（SVPWM）算法，用于三相逆变器的控制。
 * 该函数根据输入的 α-β 坐标系下的电压矢量，计算出三相 PWM 波形的占空比。
 *
 * @param alpha α 轴电压分量 0 ~ 1.0
 * @param beta  β 轴电压分量 0 ~ 1.0
 * @param d     输出数组，保存三相 PWM 占空比，分别为 d[0]、d[1]、d[2]
 *
 * @note 本函数假设输入电压矢量已归一化到 [0, 1] 区间。
 * @note 使用七段式对称调制方式计算占空比。
 */
void SVPWM(float alpha, float beta, float *d)
{
    // 1. 防止超调
    alpha *= 0.866;
    beta *= 0.866;

    //  2. 计算中间变量
    float X = beta * 2.0f / sq3;
    float Y = alpha - beta / sq3;
    float Z = alpha + beta / sq3;

    // printf("X:%f Y:%f Z:%f\n", X, Y, Z);

    // 3. 扇区判断与统一计算
    int sector = (X >= 0) ? ((Y < 0) ? ((Z >= 0) ? 2 : 3) : 1)
                          : ((Y >= 0) ? ((Z >= 0) ? 6 : 5) : 4);

    float T1, T2;
    // clang-format off
    switch (sector) {
        case 1: T1 =  Y;  T2 =  X;  break;
        case 2: T1 = -Y;  T2 =  Z;  break;
        case 3: T1 =  X;  T2 = -Z;  break;
        case 4: T1 = -X;  T2 = -Y;  break;
        case 5: T1 = -Z;  T2 =  Y;  break;
        case 6: T1 =  Z;  T2 = -X;  break;
    }
    // clang-format on
    // printf("T1:%f T2:%f\n", T1, T2);
    // 4. 计算占空比（七段式对称调制）
    float T0 = 1.0f - T1 - T2;
    float d120, d20, d0;
    d120 = T1 + T2 + T0 / 2; // Ta
    d20 = T2 + T0 / 2;       // Tb
    d0 = T0 / 2;             // Tc
                             // clang-format off
    switch (sector) {
        case 1: d[0] = d120;  d[1]=d20;	  d[2]=d0;   break;
        case 2: d[0] = d20;	  d[1]=d120;  d[2]=d0;   break;
        case 3: d[0] = d0;	  d[1]=d120;  d[2]=d20;  break;
        case 4: d[0] = d0;	  d[1]=d20;	  d[2]=d120; break;
        case 5: d[0] = d20;	  d[1]=d0;	  d[2]=d120; break;
        case 6: d[0] = d120;  d[1]=d0;	  d[2]=d20;  break;
    }
                             // clang-format on
}

void FOC_Calibrate_Angle(void)
{
    float angle1, angle1e = 0, angle2 = 0;
    float v[3];
    float alpha, beta, d = 1.2, q = 0;
    float erro;
    // 读取 theta_offset 到 theta_offset_temp
    // theta_offset_temp = theta_offset;
    flash_read(theta_offset_addr, (uint16_t *)&theta_offset_temp, 2);
    // printf("\ntheta_offset: %f\n", theta_offset_temp);
    // 确保电机处于静止状态

    // 读取初始角度
    MT6701_get_angle(NULL, &angle1);
    // printf("angle1: %f\r\n", angle1);
    wk_delay_ms(10);

    // 输出校准后的角度
    // printf("angle1 + offset: %f\r\n", angle1 + theta_offset_temp); // +偏移角度
    angle1e = -(angle1 + theta_offset_temp) * 7.0f;                // 求电角度
    // printf("angle1e: %f\r\n", angle1e);
    FOC_park_transform(TRANFORM_INVERSE, &angle1e,
                       &alpha, &beta, &d, &q); // 逆变换到Alpha-Beta坐标系
    // printf("d:%f, q:%f => alpha:%f, beta:%f\r\n", d, q, alpha, beta);
    SVPWM(alpha, beta, v); // 计算调制占空比
    // printf("v:%f, %f, %f\r\n", v[0], v[1], v[2]);

    MotorDriver_set_volatge_three(v[0], v[1], v[2]); // 输出电压
    gpio_bits_reset(GPIOC, GPIO_PINS_13);
    wk_delay_ms(200);
    gpio_bits_set(GPIOC, GPIO_PINS_13);

    // 再次读取角度
    MT6701_get_angle(NULL, &angle2);
    // printf("angle2: %f\r\n", angle2);
    wk_delay_ms(10);

    // 停止电机输出
    MotorDriver_set_volatge_three(0, 0, 0);

    // 计算误差值
    erro = angle1 - angle2;
    // printf("erro: %f\r\n", erro);
    if (fabs(erro) > erro_limit)
    {
        theta_offset_temp += erro; // 更新偏移
        // 归一化到[0, 2PI)
        theta_offset_temp = fmodf(theta_offset_temp, _2PI / 7);
        // 将更新后的偏移写回到 flash
        flash_write(theta_offset_addr, (uint16_t *)&theta_offset_temp, 2);
        // printf("Angle calibration error: %f, updated offset: %f\n\n", erro, theta_offset_temp);
    }
}

void FOC_angle(void)
{
    float theta = 0.0, thetan, angle = 0.0;
    float alpha = 0, beta = 0, d = 2, q = 0;
    float v[3];

    for (theta = 0.0; theta < _2PI * 7.0f; theta += 0.1f)
    {
        thetan = -theta;
        FOC_park_transform(TRANFORM_INVERSE, &thetan, &alpha, &beta, &d, &q);
        SVPWM(alpha, beta, v);                           // 计算空间矢量脉宽调制占空比
        MotorDriver_set_volatge_three(v[0], v[1], v[2]); // 输出电压
        wk_delay_ms(10);
        MT6701_get_angle(NULL, &angle);
        angle -= (0.135f);
        if (angle < 0)
            angle += _2PI;
        thetan = theta + _2PI;
        if (thetan > _2PI * 7.0f)
            thetan -= _2PI * 7.0f; // 保持在 [0, 2PI*7]
        printf("d:%.3f,%.3f\n", thetan, angle * 7.0f);
    }
    MotorDriver_set_volatge_three(0, 0, 0);
}
